#include "FSM.h"

FSM_t FSM;

/* 状态切换 */
static inline void FSM_ChangeState()
{
    /* 当前状态为下一个状态 */
    FSM.currentState = FSM.nextState;
    /* 设置当前函数指针 */
    switch (FSM.nextState)
    {
    case PASSIVE:
        FSM.enter = Passive_Enter;
        FSM.run = Passive_Run;
        FSM.exit = Passive_Exit;
        break;

    case FALL:
        FSM.enter = Fall_Enter;
        FSM.run = Fall_Run;
        FSM.exit = Fall_Exit;
        break;

    case FIXEDSTAND:
        FSM.enter = FixedStand_Enter;
        FSM.run = FixedStand_Run;
        FSM.exit = FixedStand_Exit;
        break;

    case FREESTAND:
        FSM.enter = FixedStand_Enter;
        FSM.run = FixedStand_Run;
        FSM.exit = FixedStand_Exit;
        break;

    case TROTING:
        FSM.enter = Troting_Enter;
        FSM.run = Troting_Run;
        FSM.exit = Troting_Exit;
        break;

    case WALKING:
        FSM.enter = Walking_Enter;
        FSM.run = Walking_Run;
        FSM.exit = Walking_Exit;
        break;

    case JUMP:
        FSM.enter = Jump_Enter;
        FSM.run = Jump_Run;
        FSM.exit = Jump_Exit;
        break;

    case JUMP2:
        FSM.enter = Jump2_Enter;
        FSM.run = Jump2_Run;
        FSM.exit = Jump2_Exit;
        break;

    case JUMPL:
        FSM.enter = JumpL_Enter;
        FSM.run = JumpL_Run;
        FSM.exit = JumpL_Exit;
        break;
    }
}

/* 有限状态机初始化 */
inline void FSM_Init(FSMState_e state)
{
    FSM.currentState = state;
    FSM.nextState = state;
    FSM_ChangeState();
    FSM.enter();
    FSM.mode = NORMAL;
}

/* 有限状态机运行 */
inline void FSM_Run(void)
{
    if (FSM.mode == NORMAL)
    {
        FSM.run(); // In normal mode, run current state.
    }
    else if (FSM.mode == CHANGE)
    {
        FSM.exit(); // In changing mode, run the exit function for previous state,
        FSM_ChangeState();
        FSM.enter(); // and run the enter function for next state.
        FSM.mode = NORMAL;
        FSM.run(); // then run the run function for current state.
    }
}

/* 有限状态机设置状态 */
inline void FSM_SetState(FSMState_e state)
{
    FSM.mode = CHANGE;
    FSM.nextState = state;
}
